# ==================== 系统描述 ====================
# GPS精度为4m
# 加速度计的精度为 1cm/s^2，且有 3cm/s^2 的偏移
# 真实的加速度为 2cm/s^2
# 数据采集频率皆为 10Hz

# ================== 模拟产生数据 ==================
clear all
rng(0);                                 # 设置随机数种子
dt = 0.1;                               # 0.1s采集一次数据
T = 0:dt:100;                           # 0~100s
N = length(T);                          # 采样点数

a = 2e-2;                               # 真实加速度
s = 1/2 * a * T.^2;                     # 真实位移（实际上不可知）
v = a * T;                              # 真实速度


aa = a + 1e-2*randn(size(s)) + 3e-2;    # 加速度计测量数据，有噪声，有偏移1
R = 4;                                  # GPS 测量精度
z = s + sqrt(R)*randn(size(s));         # GPS 测量数据，开根号得到
#Rv = 0.1;
#Zv = v + sqrt(Rv)*randn(size(s));
# ================= 变量定义与初始化 =================
A = [1 dt; 0 1];                        # 状态转移矩阵
H = [1 0];                              # 转换矩阵
B = [1/2*dt^2; dt];                     # 输入控制矩阵
Q = [0.001 0; 0 0.01];                  # 过程噪声协方差，估计一个
##
P = eye(2);                             # 初始值为 1（可为非零任意数）

##
x = zeros(2, N);                        # 存储滤波后的数据，分别为位移、速度信息
k = 1;                                  # 采样点计数

# ================= 卡尔曼滤波过程 ==================
for t = dt:dt:100
    k = k + 1
    x(:,k) = A * x(:,k-1) + B*aa(k);                # 卡尔曼公式1
    P = A * P * A' + Q;                         # 卡尔曼公式2
    K = P*H' * inv(H*P*H' + R);                 # 卡尔曼公式3
    x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k));    # 卡尔曼公式4
    P = (eye(2)-K*H) * P;                       # 卡尔曼公式5
end

# ==================== 结果绘图 =====================
figure(1);
plot(T, z(1,:),'b.');hold on;                   # GPS测量数据
plot(T, x(1,:),'r.');                           # 滤波后数据
plot(T, s ,'k-');                               # 绘制真实值
legend('滤波前','滤波后','理想值');              # 标注
xlabel('时间: s');
ylabel('距离: m');hold off;
axis([0 100 -20 120])
text(2, 110, ['滤波前方差: ' num2str(var(z - s))])            # 滤波前方差
text(2, 100, ['滤波后方差: ' num2str(var(x(1,:) - s))])       # 滤波后方差

figure(2);
plot(T(1:end-1), (x(1,2:end)-x(1,1:end-1))/dt, 'b.');hold on# 将滤波后位移差分结果
plot(T, x(2,:), 'r.');                                      # 滤波估计速度
plot(T, a*T, 'k-');                                         # 真实速度
legend('滤波位移差分','卡尔曼估计值','真实值')
xlabel('时间: s');
ylabel('速度: m');hold off;
axis([0 100 -8 10])
text(2, 9, ['位移差分方差: ' ...
    num2str(var((x(1,2:end)-x(1,1:end-1))/dt - a*T(1:end-1)))])  # 滤波前方差
text(2, 7.6, ['速度滤波方差: ' ...
    num2str(var(x(2,:) - a*T))])       # 滤波后方差

figure(3);
plot(T, v ,'k-');                               # 绘制真实值
legend('滤波前','滤波后','理想值');              # 标注
xlabel('时间: s');
ylabel('速度: v');hold off;
axis([0 100 -10 10])
